using System;
using System.Collections;
using System.Collections.Generic;
using System.Text;
using System.Threading;
using System.Windows.Forms;

namespace MyRobot.Nodes.Behaviors
{
    public class Roam : ProcessNode
    {
        #region Declarations
        static int scale = 1;
        double courage = 1;
        int leftThreshold = 25 * scale;
        int rightThreshold = 25 * scale;
        int leftRightThreshold = 30 * scale;
        int leftCheck, rightCheck;
        int forwardThreshold = 70 * scale;
        int reverseThreshold = 50 * scale;
        int turnThreshold = 50 * scale;
        int displaceThreshold = 30 * scale;
        int widthThreshold = 120 * scale;
        int tooClose = 180 * scale;
        int pathAreaThreshold = 10000;
        int RR_Highest_x, RR_Highest_y, RR_Mid_x, RR_Blob_COG_x, RR_Blob_COG_y;
        int RR_Max_radius, RR_Blob_area;
        int RovioLeftTick, RovioRightTick, RovioRearTick;
        int RovioSpeed = 9;
        int RovioTurnSpeed = 10;
        string lastAction = "";
        ArrayList SensorFlags = new ArrayList();
        SortedList RovioStatus = new SortedList();
        #endregion

        public new class PropertyClass : ProcessNode.PropertyClass
        {
        }

        public Roam(string Name) : base (Name)
        {
            this.Properties.UpdateInterval = 50;
        }

        public override void Run_Node()
        {
            while (true)
            {
                // Check to be sure we have a connection.
                if (MyRobot.myNodes.RoboRealm.Properties.ImageWidth / 320 == 0)
                {
                    Thread.Sleep(100);
                    continue;
                }

                courage = (-0.3 / 5) * MyRobot.RovioCourage + 1.3; 

                RovioSpeed = MyRobot.RovioSpeed;
                RovioTurnSpeed = MyRobot.RovioTurnSpeed;

                RR_Blob_COG_x = MyRobot.myNodes.RoboRealm.Properties.COG.X;
                RR_Blob_COG_y = MyRobot.myNodes.RoboRealm.Properties.COG.Y;
                RR_Highest_x = MyRobot.myNodes.RoboRealm.Properties.HighestPoint.X;
                RR_Highest_y = (int)MyRobot.myNodes.RoboRealm.Properties.HighestPoint.Y;
                RR_Mid_x = MyRobot.myNodes.RoboRealm.Properties.ImageWidth / 2;
                RR_Max_radius = MyRobot.myNodes.RoboRealm.Properties.BlobRadius;
                RR_Blob_area = MyRobot.myNodes.RoboRealm.Properties.BlobArea;
                RovioLeftTick = MyRobot.myNodes.RoboRealm.Properties.RovioLeftTick;
                RovioRightTick = MyRobot.myNodes.RoboRealm.Properties.RovioRightTick;
                RovioRearTick = MyRobot.myNodes.RoboRealm.Properties.RovioRearTick;

                // Set up our sensor flags.
                SensorFlags.Clear();

                // Does the Rovio IR detect an obstacle?
                if (MyRobot.myNodes.myRovio.Properties.Radar.Properties.Value == 1) SensorFlags.Add("IR obstacle");

                // Is the path corridor too narrow?
                if (RR_Max_radius < widthThreshold) SensorFlags.Add("too narrow");

                // Have we run out of floor ahead?
                if (RR_Highest_y < courage * reverseThreshold || RR_Blob_area < pathAreaThreshold) SensorFlags.Add("dead end");

                // Are we almost out of floor ahead?
                if (RR_Highest_y < courage * forwardThreshold) SensorFlags.Add("road ends ahead");

                // Are we too close to something?
                if (RR_Highest_y > tooClose) SensorFlags.Add("too close");

                // Is the current path corridor displaced left or right? (This indicates an obstacle nearby to one side.)
                if (Math.Abs(RR_Blob_COG_x - RR_Mid_x) > displaceThreshold) SensorFlags.Add("crooked path");

                // What's our best escape direction.
                leftCheck = RR_Mid_x - leftThreshold;
                rightCheck = RR_Mid_x + rightThreshold;

                if (RR_Highest_x <= leftCheck) SensorFlags.Add("escape left");
                else SensorFlags.Add("escape right");

                // Is there a preferred path to the left or right?
                if (RR_Highest_y - RR_Blob_COG_y > turnThreshold && RR_Blob_COG_y < leftRightThreshold)
                {
                    if (RR_Highest_x <= leftCheck)
                    {
                        SensorFlags.Add("turn left");
                    }
                    else if (RR_Highest_x >= rightCheck)
                    {
                        SensorFlags.Add("turn right");
                    }
                }

                // Now act accordingly.

                // Make sure we don't get trapped in a left-right oscillation.
                if (SensorFlags.Contains("escape right") && lastAction == "left")
                {
                    SensorFlags.Remove("escape right");
                    SensorFlags.Add("escape left");
                }
                else if (SensorFlags.Contains("escape left") && lastAction == "right")
                {
                    SensorFlags.Remove("escape left");
                    SensorFlags.Add("escape right");
                }

                // First, avoid any IR obstacles, dead ends or being too close to something.
                if (SensorFlags.Contains("IR obstacle") || SensorFlags.Contains("too close"))
                {
                    if (SensorFlags.Contains("escape right"))
                    {
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.Right, RovioTurnSpeed);
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.RotateRight, RovioTurnSpeed);
                        lastAction = "right";
                    }
                    else
                    {
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.Left, RovioTurnSpeed);
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.RotateLeft, RovioTurnSpeed);
                        lastAction = "left";
                    }
                    if (SensorFlags.Contains("IR obstacle"))
                    {
                        MyRobot.myGUI.DebugMessage(MyRobot.ActiveForm, (Control)MyRobot.controlHashtable["DebugMessages"], "IR Obstacle Detected");
                        MyRobot.myNodes.RoboRealm.SetVariable("ACTION", "IR Obstacle Detected");
                    }
                    else
                    {
                        MyRobot.myGUI.DebugMessage(MyRobot.ActiveForm, (Control)MyRobot.controlHashtable["DebugMessages"], "Too close");
                        MyRobot.myNodes.RoboRealm.SetVariable("ACTION", "Too Close");
                    }
                }
                // Now the dead ends.
                else if (SensorFlags.Contains("dead end"))
                {
                    if (SensorFlags.Contains("escape right"))
                    {
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.RotateRight, RovioTurnSpeed);
                        lastAction = "right";
                    }
                    else
                    {
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.RotateLeft, RovioTurnSpeed);
                        lastAction = "left";
                    }
                    MyRobot.myGUI.DebugMessage(MyRobot.ActiveForm, (Control)MyRobot.controlHashtable["DebugMessages"], "Dead end");
                    MyRobot.myNodes.RoboRealm.SetVariable("ACTION", "Dead End");
                }
                // Is the path ahead too narrow?
                else if (SensorFlags.Contains("too narrow"))
                {
                    if (SensorFlags.Contains("escape right"))
                    {
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.RotateRight, RovioTurnSpeed);
                        lastAction = "right";
                    }
                    else
                    {
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.RotateLeft, RovioTurnSpeed);
                        lastAction = "left";
                    }
                    MyRobot.myGUI.DebugMessage(MyRobot.ActiveForm, (Control)MyRobot.controlHashtable["DebugMessages"], "Too narrow");
                    MyRobot.myNodes.RoboRealm.SetVariable("ACTION", "Too Narrow");
                }
                // Are we running out of floor ahead?
                else if (SensorFlags.Contains("road ends ahead"))
                {
                    if (SensorFlags.Contains("escape right"))
                    {
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.RotateRight, RovioTurnSpeed);
                        lastAction = "right";
                    }
                    else
                    {
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.RotateLeft, RovioTurnSpeed);
                        lastAction = "left";
                    }
                    MyRobot.myGUI.DebugMessage(MyRobot.ActiveForm, (Control)MyRobot.controlHashtable["DebugMessages"], "Not enough room");
                    MyRobot.myNodes.RoboRealm.SetVariable("ACTION", "Not Enough Room");
                }
                // Is the current path corridor displaced left or right?
                else if (SensorFlags.Contains("crooked path"))
                {
                    if (RR_Blob_COG_x - RR_Mid_x < 0)
                    {
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.DiagonalForwardLeft, RovioSpeed);
                        MyRobot.myGUI.DebugMessage(MyRobot.ActiveForm, (Control)MyRobot.controlHashtable["DebugMessages"], "Skew left");
                        MyRobot.myNodes.RoboRealm.SetVariable("ACTION", "Skew Left");
                        lastAction = "skew left";
                    }
                    else
                    {
                        MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.DiagonalForwardRight, RovioSpeed);
                        MyRobot.myGUI.DebugMessage(MyRobot.ActiveForm, (Control)MyRobot.controlHashtable["DebugMessages"], "Skew right");
                        MyRobot.myNodes.RoboRealm.SetVariable("ACTION", "Skew Right");
                        lastAction = "skew right";
                    }
                }
                // All clear ahead so check to see if we should go left, right or straight.
                else if (SensorFlags.Contains("turn left"))
                {
                    MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.DiagonalForwardLeft, RovioSpeed);
                    MyRobot.myGUI.DebugMessage(MyRobot.ActiveForm, (Control)MyRobot.controlHashtable["DebugMessages"], "Skew left");
                    MyRobot.myNodes.RoboRealm.SetVariable("ACTION", "Skew Left");
                    lastAction = "skew left";
                }
                else if (SensorFlags.Contains("turn right"))
                {
                    MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.DiagonalForwardRight, RovioSpeed);
                    MyRobot.myGUI.DebugMessage(MyRobot.ActiveForm, (Control)MyRobot.controlHashtable["DebugMessages"], "Skew right");
                    MyRobot.myNodes.RoboRealm.SetVariable("ACTION", "Skew Right");
                    lastAction = "skew right";
                }
                else
                {
                    MyRobot.myNodes.RoboRealm.RovioDrive(MyRobot.RovioDriveCodes.Forward, RovioSpeed);
                    MyRobot.myNodes.RoboRealm.SetVariable("ACTION", "Go Straight");
                    lastAction = "straight";
                }
                Thread.Sleep(this.Properties.UpdateInterval);
            }
        }
    }

}
